#include "my_predict/Kalman_filter.h"


using namespace std;
using namespace cv;


//状态向量长什么样？
//目标是一个二位空间的点，所以需要4个变量来描述(x,y,Vx,Vy)
Kalman_filter::Kalman_filter(){
    //协方差矩阵的过程噪声，
//    Q = (Mat_<double>(4,4) <<
//         0.005, 0, 0, 0,
//         0, 0.005, 0, 0,
//         0, 0, 0.005, 0,
//         0, 0, 0, 0.005);
    Q=(Mat_<double>(4,4) <<
       1,0,0,0,
       0,1,0,0,
       0,0,1,0,
       0,0,0,1);

    //测量噪声矩阵，如果是雷达的话商家会给出
    //摄像头的话商家怎么可能给
    R2 = (Mat_<double>(4, 4) <<
          1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1, 0,
          0, 0, 0, 1 );

    A = (Mat_<double>(4, 4) <<
         1, 0, 1, 0,
         0, 1, 0, 1,
         0, 0, 1, 0,
         0, 0, 0, 1 );

    //测量矩阵，将状态向量的量纲转换为与测量值一样
    //摄像头的测量值为（x,y，Vx,Xy）,所以H应该是2*4的向量
    H2 = (Mat_<double>(4, 4) <<
          1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1, 0,
          0, 0, 0, 1 );

    //协方差矩阵，随便写，随着更新趋于准确值
    //但是初始化的时候最好根据x的准确性写出.
    po2 = (Mat_<double>(4,4) <<
          1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1000, 0,
          0, 0, 0, 1000);  //0.00623 don't worried, because it can update

    Z2 = (Mat_<double>(4, 1) << 0, 0, 0, 0);

    po_pre2 = po2;

    x_pre2 = (Mat_<double>(4,1)<< 0,0,0,0);

    k_g2 = (Mat_<double>(4, 4) <<
          0, 0, 0, 0,
          0, 0, 0, 0,
          0, 0, 0, 0,
          0, 0, 0, 0 );

    x_guji2 = (Mat_<double>(4,1)<< 0,0,0,0);

}

vector<double> Kalman_filter::GetnowPoint(double x, double y){
    if (x_list.size() < 2){
        x_list.push_back(x);
    }else{
        x_list.push_back(x);
        x_list.pop_front();
    }

    if (y_list.size() < 2){
        y_list.push_back(y);
    }else{
        y_list.push_back(y);
        y_list.pop_front();
    }

    Vx = x_list.back() - x_list.front();
    Vy = y_list.back() - y_list.front();

    nowpoints2.clear();
    Mat I = Mat::eye(4,4, CV_64F);

    Z2 = (Mat_<double>(4, 1) << x, y, Vx, Vy);  //测量值
    x_pre2 = A * x_guji2 ;  //状态向量，无控制向量
    po_pre2 = A * po2 * A.t() + Q;  //协方差矩阵的预测量
    Y=Z2 - H2 * x_pre2;     //估计值与测量值的差
    S=H2 * po_pre2 * H2.t() + R2;
    k_g2 = po_pre2 * H2.t() * S.inv();//卡尔曼增益，代表Y的权重
    x_guji2 = x_pre2 + k_g2* Y;     //输出的预测值
    nowpoints2.push_back( x_guji2.at<double>(0,0) );
    nowpoints2.push_back( x_guji2.at<double>(1,0) );
    nowpoints2.push_back( x_guji2.at<double>(2,0) );
    nowpoints2.push_back( x_guji2.at<double>(3,0) );

    po2 = (I - k_g2 * H2) * po_pre2;    //更新协方差矩阵
    return nowpoints2;
}

void Kalman_filter::SetPredictError(double predict_error){
    this->predict_error = predict_error;
    Q = Mat::eye(Size(4, 4), CV_64FC1) * predict_error;

}

void Kalman_filter::SetComputeError(double compute_error){
    this->compute_error = compute_error;
    R2 = Mat::eye(Size(4, 4), CV_64FC1) * compute_error;
    //传感器误差
}

void Kalman_filter::ReInitAllState(){
    Q = Mat::eye(Size(4, 4), CV_64FC1) * predict_error;
    R2 = Mat::eye(Size(4, 4), CV_64FC1) * compute_error;

    A = (Mat_<double>(4, 4) <<
         1, 0, 1, 0,
         0, 1, 0, 1,
         0, 0, 1, 0,
         0, 0, 0, 1 );

    //测量矩阵，将状态向量的量纲转换为与测量值一样
    //摄像头的测量值为（x,y，Vx,Xy）,所以H应该是2*4的向量
    H2 = (Mat_<double>(4, 4) <<
          1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1, 0,
          0, 0, 0, 1 );

    //协方差矩阵，随便写，随着更新趋于准确值
    //但是初始化的时候最好根据x的准确性写出.
    po2 = (Mat_<double>(4,4) <<
          1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1000, 0,
          0, 0, 0, 1000);  //0.00623 don't worried, because it can update

    Z2 = (Mat_<double>(4, 1) << 0, 0, 0, 0);

    po_pre2 = po2;

    x_pre2 = (Mat_<double>(4,1)<< 0,0,0,0);

    k_g2 = (Mat_<double>(4, 4) <<
          0, 0, 0, 0,
          0, 0, 0, 0,
          0, 0, 0, 0,
          0, 0, 0, 0 );

    x_guji2 = (Mat_<double>(4,1)<< 0,0,0,0);
    x_list.clear();
    y_list.clear();
}
